| ROS联合Webots仿真 | 您所在的位置：网站首页 › ros 控制器 › ROS联合Webots仿真 |
ROS联合Webots仿真
|
ROS联合Webots仿真
文章开始之前首先提出以下几点： 本文的目的：通过在Webots中搭建仿真环境使读者快速入门Webots，能使用Webots开展ROS下的仿真工作，为实际项目的落地打好基础。笔者的工作内容：整合关于Webots的一些较好的入门资料，通过跑通一些例程：一如建图、导航等，与读者一起学习并且实操Webots这款仿真软件，后续的工作笔者也在学习当中，敬待更新；为什么要学Webots：Webots是一款类似于Gazebo的物理仿真软件，其优点在于集成了大量的场景文件，同时在社区内有许多开源的资源可使用（当然这些优点其实大部分仿真软件都具备，但笔者一直认为：会用工具本身也是一种能力，多掌握一种工具不是一件坏事，这样面对实际的问题也有多种工具可供选择）令我感觉最深刻的是，需要自己编写控制器代码，这点虽然不是新手友好型，但是这点可以当作一种算法验证的工具。注：本文Ubuntu系统是16.04，ros版本为kinetic，所需的Webots版本为2020a rev1。 一、Webots的安装和配置 1. 下载并安装Webots由于git和Webots官网下载速度较慢，这里附上百度网盘链接（实际上读者也可以尝试前面两种下载方式，因为网盘下载速度更慢…）： 链接：https://pan.baidu.com/s/1uAafKhKggayauXdWxTbXJA 提取码：zlg7 下载完成后进入文件所在目录，打开终端输入以下指令解压安装包： dpkg -i webots_2020a-rev1_amd64.deb 安装webots_ros功能包： sudo apt install ros-kinetic-webots-ros 2. 配置Webots 安装完成后进入/usr/local/webots/projects/languages/文件夹下，里面有四个文件夹。
在文件最后一行加入下列代码，点击保存： export WEBOTS_HOME=/usr/local/webots 进入catkin_ws/src打开终端编译： catkin_make 打开官方例程pioneer3at机器人： roslaunch webots_ros pioneer3at.launch
至此也完成了Webots的安装与配置。 二、搭建仿真环境上一章节中我们安装并配置了Webots，并且运行了官方的Webots例程。本章将为读者讲解如何搭建一个简单的仿真环境，在Webots中搭建一个属于自己的双轮差速机器人。 1. 创建一个新世界 step 1：打开 Webots，设置中文模式：按下Win键，搜索Webots，为了方便起见，建议把图标固定到任务栏；菜单栏选择Tools->Preferences进入如下界面把Language设置为中文。 菜单栏选择文件->新世界，创建一个新的地图。 左侧窗口我们称之为场景树，在场景树中点击worldinfo，可以看到有个“+”按钮被激活，点击该按钮。 step 4：在弹出的窗口中为环境添加节点：找到PROTO nodes(Webots Projects)->objects->backgrounds，将展开后的两个节点均Add进新建的世界。 step 5：添加地板：找到PROTO nodes(Webots Projects)->objects->floors，将展开后的RectangleArena(solid)加入世界中。 以上操作过后，显示的世界如下图：  双击appearance添加颜色纹理Base_nodes->Appearance，双击geometry 添加形状Base_nodes->Box；双击appearance->material添加金属属性，可以修改其节点下的diffusecolor属性更改颜色；在geometry节点添加几何属性，设置车体大小：x=0.5m、y=0.15m、z=0.5m，在translation节点中将其在y上坐标设置高一些，方便后面加轮子。（注意：translation节点的参数值都是相对于世界坐标系的物体坐标）现在得到的车身如下图所示：  在Robot节点下双击boundingObject：因为上面DEF了body，直接use body即可；同理双击Robot节点下重力属性physics（只要创建出physics子节点即可）
step 2：加入车轮：
在Robot->children下创建一个Base_nodes->HingeJoint节点,DEF为left_wheel，该节点下会出现三个节点，如下图所示：  在HingeJoint->jointParameters节点下创建子节点HingeJointParameters；
该节点主要用来设置： 铰链轴的方向（axis，电机旋转轴的平行向量）铰链轴的位置（anchor，表示电机转轴相对于父节点的坐标偏移量）弹簧系数（springConstant）阻尼系数系数（dampingConstant）静摩擦系数（staticFriction） 点击查看–>可选显示–>show joint Axes或者同时按下ctrl+F5，可以显示出旋转轴，方便我们调整旋转轴及旋转原点；在HingeJoint->device节点下添加电机Rotatioal Motor,Rotatioal Motor->name命名为left_motor；该节点主要用来设置： 电机加速度（acceleration）消耗因数（comsuptionFactor）PID参数（controlPID，从左到右为P、I、D，位置控制模式）电机最大速度（maxVelocity）电机最大转矩（maxTorque） 在HingeJoint->endPoint节点下，增加Base_nodes->soild节点，该节点主要是设置电机模型以及其他功能参数，之后在soild->children节点下创建一个shape节点，在shape节点下设置轮子的形状为Cylinder；通过调整translation和rotation，将轮子设置到合适的位置，轮子大小设置为（height=0.08 radius=0.12）如图： 接下来通过调整axis和 anchor，将电机旋转轴调整到正确位置，比如我这里电机的位置相对于车体（x=0.31、y=0、z=0）,于是我将旋转轴相对于父节点(body)的位置通过anchor调整至此，并且修改axis的各个值，观察到我的轮子是绕x轴旋转，所以将axis设置为（1 0 0）。可以看到，轮子处有一个黑线（如上图中所示），表示旋转轴在这里，黑线表示的就是电机旋转轴的位置和方向。笔者在这里设置的（axis=2 0 0）m和（anchor=0.31 0 0）m最后，在HingeJoint->endPoint节点下给轮子增加碰撞属性boundingObject(与轮子大小一致)及重力属性physics（创建一个physics子节点）接下来复制出另一个轮子，按照上面的方法调整axis和anchor,并且更改电机name为right_motor
以上操作完成后，模型变为如下图所示: 
step 4：加入激光雷达
Robot->children节点下双击添加PROTO nodes—>devices->sick->SickLms291(lidar)；
在sick（Sensor Intelligence）公司官网上可以查到SickLms291这款雷达的各项参数： 
step 5：加入GPS和IMU
Robot->children下添GPS"gps"和InertialUnit"inertial unit" 至此，小车的模型我们就建立完成，点击窗口上的运行按钮，便可以看到小车由于自身的重力属性落在了地板上。 前面两小节我们已经完成世界模型和小车模型的搭建，本节在此基础上需要创建一个控制器，通过ROS来实现对小车的控制。在实现控制前，我们需要一些前置的工作。 （1）配置功能包 命令行创建功能包： cd ~/.catkin_ws/src catkin_create_pkg webots_demo std_msgs rospy roscpp sensor_msgs 编译： cd ~/.catkin_ws catkin_make 把/usr/local/webots/projects/default/controllers/ros/include/下的srv和msg文件夹复制到catkin_ws/src/webots_demo下；进入webots_demo的CMakeList.txt文件进行配置，将附录中[1]段代码粘贴进文件并保存；进入同目录下webots_demo的package.xml文件，加入两行： message_generation message_runtime 同目录下建立world、launch和controller文件夹，controller下建立my_controller文件夹；将上一小节建立的世界文件webots_map.wbt放入world中；在launch文件夹中创建webots.launch，代码如下：其实也可以手动连接ROS master，开启终端输入如下指令后，用Webots打开webots_demo下的webots_map.wbt世界文件即可 roscore 用Webots打开世界文件，展开Robot节点，双击controller，将控制器从void改为ros，controllerArgs改为--name=robot；
（2）编写控制器（键盘控制小车）
cpp文件代码见附录中[2]；将该文件命名为my_controller.cpp，分别复制到catkin_ws/src/webots_demo/controller/my_controller和catkin_ws/src/webots_demo/src下在catkin_ws/src/webots_demo/CMakeList.txt最后补充：
add_executable(my_controller src/my_controller.cpp)
add_dependencies(my_controller webots_ros_generate_messages_cpp)
target_link_libraries(my_controller ${catkin_LIBRARIES})
这三行使得我们编译功能包后会生成对应的可执行文件。 三、ROS联合Webots进行仿真在前面两章的基础之上，我们已经完成了仿真环境的搭建，本章节将动手实操，用我们写好的键盘控制器来控制仿真环境中的机器人，并对其中的一些数据进行分析。 1. 运行仿真环境 编译功能包webots_demo： cd ~/.catkin_ws catkin_make 运行webots.launch文件，启动世界： roslaunch webots_demo webots.launch这里如果出现一些问题，例如笔者出现了启动launch文件后Webots闪退，可以打开launch文件检查一下做出相应的修改，也可以用第二章第3小节里提到的手动连接ROS master的方法。此处旨在让仿真环境连接上ROS，才能进行后续操作。 世界文件运行起来后，运行控制器，启动键盘控制节点： rosrun webots_demo my_controller这个时候鼠标点击仿真环境地图内，按下键盘上的方向键即可操控小车。 查看rosservice： rosservice list可以看到如下服务： 终端显示： success: True 查看rostopic： rostopic list
这里可以看到：键盘节点、雷达均有话题发布 键盘话题：/robot/keyboard/key 雷达发布的话题：/robot/Sick_LMS_291/laser_scan/layer0、 /robot/Sick_LMS_291/range_image 为了可视化的看到雷达的数据，我们可以打开rviz： rviz将左手Displays栏里的Fixed Frame改为robot/Sick_LMS_291 点击Add->By topic订阅话题/robot->/Sick_LMS_291->/laser_scan->/layer0->LaserScan和/robot->/Sick_LMS_291->/range_image->Image 可以看到雷达扫描的数据被可视化： 在前文的基础之上，我们可以像以往在Gazebo里仿真一样，实现移动机器人的自主导航。前文加入了三个传感器： GPS：用于Webots中移动小车的定位； IMU：利用获取的惯性信息来补偿小车位姿； Sick_LMS_291：识别障碍信息，用于地图构建和避障。 具体实现小车导航的仿真，可转到Reference[1]中的https://blog.csdn.net/xiaokai1999/article/details/112596613 这里说明一下，笔者也是一步一步跟着这位博主学习操作Webots的，本文中有许多的步骤，都是与这位博主的相同，但是笔者都有自己去动手操作，毕竟这东西光看是学不会的，更别提看着教程操作有的地方会出现不同的状况了。所以，我尽量中和了一些关键的博客，部分文字是自己总结出来的，方便读者理解和上手。 在跟随这篇博文操作后，可以查看到tf_tree： 2021.10.17更新 不足与展望 后续希望自己搭建出一个地图，用gmapping或者cartographer进行建图；后续可实现Lidar Slam的仿真；后续可实现VSlam的仿真；后续希望搭建机械臂相关仿真环境；整个项目希望利用git进行管理。 Reference[1] CSDN.锡城筱凯.ROS联合webots实战案例目录 [2] CSDN.JameScottX.Webots 舵轮底盘小教程 附录 catkin_ws/src/webots_demo/CMakeList.txt cmake_minimum_required(VERSION 2.8.3) project(webots_demo) find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation) ####################################### ## Declare ROS messages and services ## ####################################### ## Generate messages in the 'msg' folder add_message_files( FILES BoolStamped.msg Float64Stamped.msg Int32Stamped.msg Int8Stamped.msg RadarTarget.msg RecognitionObject.msg StringStamped.msg ) ## Generate services in the 'srv' folder add_service_files( FILES camera_get_focus_info.srv camera_get_info.srv camera_get_zoom_info.srv display_draw_line.srv display_draw_oval.srv display_draw_pixel.srv display_draw_polygon.srv display_draw_rectangle.srv display_draw_text.srv display_get_info.srv display_image_copy.srv display_image_delete.srv display_image_load.srv display_image_new.srv display_image_paste.srv display_image_save.srv display_set_font.srv field_get_bool.srv field_get_color.srv field_get_count.srv field_get_float.srv field_get_int32.srv field_get_node.srv field_get_rotation.srv field_get_string.srv field_get_type.srv field_get_type_name.srv field_get_vec2f.srv field_get_vec3f.srv field_import_node.srv field_import_node_from_string.srv field_remove_node.srv field_remove.srv field_set_bool.srv field_set_color.srv field_set_float.srv field_set_int32.srv field_set_rotation.srv field_set_string.srv field_set_vec2f.srv field_set_vec3f.srv get_bool.srv get_float_array.srv get_float.srv get_int.srv get_string.srv get_uint64.srv get_urdf.srv gps_decimal_degrees_to_degrees_minutes_seconds.srv lidar_get_frequency_info.srv lidar_get_info.srv lidar_get_layer_point_cloud.srv lidar_get_layer_range_image.srv motor_set_control_pid.srv mouse_get_state.srv node_add_force_or_torque.srv node_add_force_with_offset.srv node_get_center_of_mass.srv node_get_contact_point.srv node_get_field.srv node_get_id.srv node_get_number_of_contact_points.srv node_get_name.srv node_get_orientation.srv node_get_parent_node.srv node_get_position.srv node_get_static_balance.srv node_get_status.srv node_get_type.srv node_get_velocity.srv node_remove.srv node_reset_functions.srv node_move_viewpoint.srv node_set_visibility.srv node_set_velocity.srv pen_set_ink_color.srv range_finder_get_info.srv receiver_get_emitter_direction.srv robot_get_device_list.srv robot_set_mode.srv robot_wait_for_user_input_event.srv save_image.srv set_bool.srv set_float.srv set_float_array.srv set_int.srv set_string.srv skin_get_bone_name.srv skin_get_bone_orientation.srv skin_get_bone_position.srv skin_set_bone_orientation.srv skin_set_bone_position.srv speaker_is_sound_playing.srv speaker_speak.srv speaker_play_sound.srv supervisor_get_from_def.srv supervisor_get_from_id.srv supervisor_movie_start_recording.srv supervisor_set_label.srv supervisor_virtual_reality_headset_get_orientation.srv supervisor_virtual_reality_headset_get_position.srv ) ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES std_msgs sensor_msgs ) ################################### ## catkin specific configuration ## ################################### catkin_package( CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime ) ########### ## Build ## ########### include_directories( ${catkin_INCLUDE_DIRS} ) ## Instructions for keyboard_teleop node ############# ## Install ## ############# catkin_ws/src/webots_demo/controller/my_controller/my_controller.cpp #include #include #include "ros/ros.h" #include #include #include using namespace std; #define TIME_STEP 32 //时钟 #define NMOTORS 2 //电机数量 #define MAX_SPEED 2.0 //电机最大速度 ros::NodeHandle *n; static int controllerCount; static vector controllerList; ros::ServiceClient timeStepClient; //时钟通讯客户端 webots_ros::set_int timeStepSrv; //时钟服务数据 ros::ServiceClient set_velocity_client; //速度设置客户端 webots_ros::set_float set_velocity_srv; //速度设置数据 ros::ServiceClient set_position_client; //位置设置客户端 webots_ros::set_float set_position_srv; //位置设置数据 double speeds[NMOTORS]={0.0,0.0}; //电机速度值 0～100 // 匹配电机名 static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"}; /******************************************************* * Function name ：updateSpeed * Description ：将速度请求以set_float的形式发送给set_velocity_srv * Parameter ：无 * Return ：无 **********************************************************/ void updateSpeed() { for (int i = 0; i controllerCount++; controllerList.push_back(name->data);//将控制器名加入到列表中 ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str()); } /******************************************************* * Function name ：quit * Description ：退出函数 * Parameter ： @sig 信号 * Return ：无 **********************************************************/ void quit(int sig) { ROS_INFO("User stopped the '/robot' node."); timeStepSrv.request.value = 0; timeStepClient.call(timeStepSrv); ros::shutdown(); exit(0); } /******************************************************* * Function name ：键盘返回函数 * Description ：当键盘动作，就会进入此函数内 * Parameter ： @value 返回的值 * Return ：无 **********************************************************/ void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value) { // 发送控制变量 int send =0; //ROS_INFO("sub keyboard value = %d",value->data); switch (value->data) { // 左转 case 314: speeds[0] = 5.0; speeds[1] = -5.0; send=1; break; // 前进 case 315: speeds[0] = 5.0; speeds[1] = 5.0; send=1; break; // 右转 case 316: speeds[0] = -5.0; speeds[1] = 5.0; send=1; break; // 后退 case 317: speeds[0] = -5.0; speeds[1] = -5.0; send=1; break; // 停止 case 32: speeds[0] = 0; speeds[1] = 0; send=1; break; default: send=0; break; } //当接收到信息时才会更新速度值 if (send) { updateSpeed(); send=0; } } int main(int argc, char **argv) { setlocale(LC_ALL, ""); // 用于显示中文字符 string controllerName; // 在ROS网络中创建一个名为robot_init的节点 ros::init(argc, argv, "robot_init", ros::init_options::AnonymousName); n = new ros::NodeHandle; // 截取退出信号 signal(SIGINT, quit); // 订阅webots中所有可用的model_name ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback); while (controllerCount == 0 || controllerCount int wantedController = 0; cout wantedController; if (1 // position速度控制时设置为缺省值INFINITY set_position_client = n->serviceClient(string("/robot/") + string(motorNames[i]) + string("/set_position")); set_position_srv.request.value = INFINITY; if (set_position_client.call(set_position_srv) && set_position_srv.response.success) ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]); else ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]); // velocity初始速度设置为0 set_velocity_client = n->serviceClient(string("/robot/") + string(motorNames[i]) + string("/set_velocity")); set_velocity_srv.request.value = 0.0; if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1) ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]); else ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]); } // 服务订阅键盘 ros::ServiceClient keyboardEnableClient; webots_ros::set_int keyboardEnablesrv; keyboardEnableClient = n->serviceClient("/robot/keyboard/enable"); keyboardEnablesrv.request.value = TIME_STEP; if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success) { ros::Subscriber keyboardSub; keyboardSub = n->subscribe("/robot/keyboard/key",1,keyboardDataCallback); while (keyboardSub.getNumPublishers() == 0) {} ROS_INFO("Keyboard enabled."); ROS_INFO("control directions："); ROS_INFO(" ↑ "); ROS_INFO("← ↓ →"); ROS_INFO("stop：space"); ROS_INFO("Use the arrows in Webots window to move the robot."); ROS_INFO("Press the End key to stop the node."); while (ros::ok()) { ros::spinOnce(); if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) { ROS_ERROR("Failed to call service time_step for next step."); break; } ros::spinOnce(); } } else ROS_ERROR("Could not enable keyboard, success = %d.", keyboardEnablesrv.response.success); //退出时时钟清零 timeStepSrv.request.value = 0; timeStepClient.call(timeStepSrv); ros::shutdown(); return 0; } |
可以看到下面提示栏里提示：
这里设置为中文后，软件内只有部分位置被翻译过来，不过没关系，后续笔者会对用到的地方做出详细的解释。
我们可以改变场景树中的RectangleArena->floorsize：双击后改为x=5m、y=5m，方便后面进行小车的仿真。
工作范围示意图如下图所示： 
这里注意到：其扫描范围只有前方一个半圆区域，这点不同于常见的激光雷达（例如Rplidar）；
最后建议在环境中加入一些障碍物，例如墙体、箱体之类，便于后面仿真，对于此便不再操作和赘述。 此时，我们将世界保存为webots_map.wbt
在Webots中用鼠标点击一下background，然后用键盘上的方向键来控制小车运动，可以看到rviz中LaserScan图像在实时变化： 
其中：【本文地址】
公司简介
联系我们